package edu.chalmers.AV_2DPlatformerFramework.Model;

import edu.chalmers.AV_2DPlatformerFramework.Utility.Vector2D;
import edu.chalmers.AV_2DPlatformerFramework.Utility.Triple;
import edu.chalmers.AV_2DPlatformerFramework.Utility.VectorUtility;
import edu.chalmers.AV_2DPlatformerFramework.Utility.Pair;
import java.awt.geom.Line2D;
import java.util.ArrayList;
import java.util.Arrays;

/**
 * Utility class for moving GameObjects, with functionality for collision
 * detection and handling.
 * @version 1.3
 * @author Joakim Karlsson, Gustav Vidhög
 * @revised Gustav Vidhög, 20140424
 * @revised Joakim Karlsson, Gustav Vidhög, 20140501
 * @revised Joakim Karlsson, Gustav Vidhög, 20140508
 * @revised Joakim Karlsson, Gustav Vidhög, 20140511
 * @revised Gustav Vidhög, 20140522
 */
public class ObjectMover {
    public static final double EPS = Math.pow(2, -48);
   
    // Needs rework of overlap, projectObject, collides, and collidesWith.
    // Needs move to handle objects of infinite mass.
    
    /**
     * Moves a number of objects accounting for possible collisions.
     * @param moves - pairs of objects to be moved and their respective
     * accelerations.
     * @param objects - list of all objects possible to collide with.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.0
     * @since 0.1
     */
    public static void moveObjects(Pair<GameObject, Vector2D>[] moves,
            ArrayList<GameObject> objects) {
       for (Pair<GameObject, Vector2D> m : moves) {
           move(m.getFirst(), m.getSecond(), objects);
       } //end for
    } //end moveObjects
   
    /**
     * Moves an object accounting for possible collisions.
     * @param object - object to be moved.
     * @param acceleration - acceleration of object to be moved.
     * @param objects - list of all objects possible to collide with.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.0
     * @since 0.1
     * @revised Gustav Vidhög, 20140522
     */
    public static void move(GameObject object, Vector2D acceleration,
            ArrayList<GameObject> objects) {
        
        // Test whether object is still.
        if (!VectorUtility.isNull(object.getVelocity())) {
            
            // Make list of other objects.
            ArrayList<GameObject> otherObjects =
                    (ArrayList<GameObject>) objects.clone();
            otherObjects.remove(object);
            
            // Test whether this object has a physical representation.
            if (object.getHitbox() != null) {
                
                // Object has a physical representation, proceed with collision
                // detection and handling.
                
                // Create temporary translated object to use in collision
                // detection.
                GameObject tempObj = object.copy();
                tempObj.setPosition(VectorUtility.add(object.getPosition(),
                        object.getVelocity()));
                    
                // Remove objects whose bounding boxes do not overlap from list.
                sieveCollidableObjects(tempObj, otherObjects);
                
                // Collision detection.
                Triple<GameObject, Vector2D, Vector2D> collision =
                        collidesWith(tempObj, otherObjects);
                    
                // Test whether collision occured.
                if(collision == null) {
                    
                    // Collision did not occur, update position and velocity.
                    object.setPosition(VectorUtility.add(
                            object.getPosition(), object.getVelocity()));
                    object.setVelocity(VectorUtility.add(
                        object.getVelocity(), acceleration));
                    
                } else {
                    
                    // Collision occured, test whether objects should interact
                    // physically.
                    if (object.interactsWith(collision.getFirst())) {
                        
                        // Objects collided physically.
                        
                        double cr = coefficientOfRestitution(
                            object, collision.getFirst());
                        double m1 = object.getWeight();
                        double m2 = collision.getFirst().getWeight();
                        
                        //Split velocities in normal and tangential components.
                        Vector2D normal = VectorUtility.normalize(
                                collision.getSecond());
                        Vector2D v1n = VectorUtility.project(
                                object.getVelocity(), normal);
                        Vector2D v1t = VectorUtility.subtract(
                                object.getVelocity(), v1n);
                        double v1nScalar = VectorUtility.norm(v1n);
                        Vector2D v2n = VectorUtility.project(
                                collision.getFirst().getVelocity(), normal);
                        Vector2D v2t = VectorUtility.subtract(
                                collision.getFirst().getVelocity(), v2n);
                        double v2nScalar = VectorUtility.norm(v2n);
                        
                        double v1nScalarNew;
                        double v2nScalarNew;
                        
                        // Calculate new velocities normal to the edge collided
                        // with. First test whether the objects have infinite
                        // mass.
                        if (Double.isInfinite(m2)) {
                            
                            // Object collided with has infinite mass, its
                            // velocity will stay unchanged. This is prioritized
                            // over testing whether this object has infinite
                            // mass; in practice handling collisions between two
                            // infinitely massive objects should rarely be of
                            // interest.
                            v1nScalarNew = - cr * (v2nScalar - v1nScalar) +
                                    v2nScalar;
                            v2nScalarNew = v2nScalar;
                            
                        } else if (Double.isInfinite(m1)) {
                            
                            // This object has infinite mass, its velocity will
                            // stay unchanged.
                            v1nScalarNew = v1nScalar;
                            v2nScalarNew = cr * (v1nScalar - v2nScalar) +
                                    v1nScalar;
                            
                        } else {
                            
                            // Objects are both finitely massive and will change
                            // velocities after collision.
                            v1nScalarNew = (- cr * m2 * (v2nScalar - v1nScalar)
                                + m1 * v1nScalar + m2 * v2nScalar) / (m1 + m2);
                            v2nScalarNew = (cr * m1 * (v1nScalar - v2nScalar)
                                + m1 * v1nScalar + m2 * v2nScalar) / (m1 + m2);
                            
                        } // end if
                        
                        
                        // Update velocities to account for collision.
                        object.setVelocity(VectorUtility.add(
                                VectorUtility.scale(
                                        normal, v1nScalarNew), v1t));
                        collision.getFirst().setVelocity(VectorUtility.add(
                                VectorUtility.scale(
                                        normal, v2nScalarNew), v2t));
                            
                        // Call onCollision methods, update velocity to account
                        // for acceleration.
                        object.onCollision(collision.getFirst());
                        collision.getFirst().onCollision(object);
                        object.setVelocity(VectorUtility.add(
                            object.getVelocity(), acceleration));
                        
                    } else {
                        
                        // Objects crossed but does not interact physically,
                        // call onCollision methods and attempt to move this
                        // object again, ignoring the other object.
                        object.onCollision(collision.getFirst());
                        collision.getFirst().onCollision(object);
                        otherObjects.remove(collision.getFirst());
                        move(object, acceleration, otherObjects);
                        
                    } //end if
                } // if
            } else {
                
                // Object does not have a physical representation, just update
                // position and velocity.
                
                
            } // end if
        } else {
            
            // Object did not move, update velocity to account for acceleration.
            object.setVelocity(VectorUtility.add(
                    object.getVelocity(), acceleration));
            
        } // end if
    } // end move
   
    /**
     * Calculates coefficient of restitution for a collision between two
     * objects.
     * @param object1 - first colliding object.
     * @param object2 - second colliding object.
     * @return coefficient of restitution
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.0
     * @since 1.0
     */
    private static double coefficientOfRestitution(
            GameObject object1, GameObject object2) {
        return object1.getElasticity() * object2.getElasticity();
    } //end coefficientOfRestitution
    
    /**
     * Sieves list of objects for those in close proximity of object.
     * Optimization removing objects whose rectangular bounding boxes do not
     * intersect that of the object to be moved from list of objects to be
     * tested for collisions.
     * @param object - object to be moved.
     * @param otherObjects - list of objects to be sieved.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.1
     * @since 0.1
     * @revised Gustav Vidhög, 20140424
     * @revised Richard Sundquist, 20140515
     * @revised Gustav Vidhög, 20140515
     */
    private static void sieveCollidableObjects(GameObject object,
            ArrayList<GameObject> otherObjects) {
        Hitbox tempHitbox = object.getHitbox().copy();

        tempHitbox.translate(object.getPosition());

        Hitbox tempOtherHitbox;
        GameObject[] tempList = new GameObject[otherObjects.size()];
        otherObjects.toArray(tempList);
        for(GameObject otherObject : tempList) {
            if (otherObject.getHitbox() != null) {
                tempOtherHitbox = otherObject.getHitbox().copy();
                tempOtherHitbox.translate(otherObject.getPosition());
                if(!(tempHitbox.overlaps(tempOtherHitbox))) {
                    otherObjects.remove(otherObject);
                } // end if
            } else {
                otherObjects.remove(otherObject);
            } // end if
        } // end for
    } // end sieveCollidableObjects
   
    /**
     * Finds collisions between object to be moved and any other object, if any.
     * @param object - object to be moved.
     * @param objects - list of all objects possible to collide with.
     * @return object collided with, unit vector perpendicular to the edge
     * collided against, and minimal translative correction required to separate
     * {@code object} from object collided against, if collision occurred; null
     * otherwise.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.0
     * @since 0.1
     */
    private static Triple<GameObject, Vector2D, Vector2D> collidesWith(
            GameObject object, ArrayList<GameObject> objects) {
        Triple<GameObject, Vector2D, Vector2D> result = null;
        Pair<Vector2D, Vector2D> temp;
        for (GameObject otherObject : objects) {
            temp = collides(object, otherObject);
            if(temp != null) {
                if(result == null ||
                        VectorUtility.norm(temp.getSecond()) >
                        VectorUtility.norm(result.getThird())) {
                    result = new Triple<>(
                            otherObject, temp.getFirst(), temp.getSecond());
                } // end if
            } // end if
         } // end for
         return result;
    } // end collidesWith
   
    /**
     * Finds collision between object to be moved and another, if any.
     * @param object - object to be moved.
     * @param otherObject - object to find eventual collision with.
     * @return unit vector perpendicular to the edge collided against, and
     * minimal translative correction required to separate {@code object} from
     * {@code otherObject}, if collision occurred; null otherwise.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.3
     * @since 1.0
     * @revised Joakim Karlsson, Gustav Vidhög, 20140508
     * @revised Joakim Karlsson, Gustav Vidhög, 20140511
     * @revised Gustav Vidhög, 20140515
     */
    private static Pair<Vector2D, Vector2D> collides(
            GameObject object, GameObject otherObject) {
        ArrayList<Vector2D> axes = new ArrayList<>();
        ArrayList<Line2D.Double> edges = new ArrayList<>(
                Arrays.asList(object.getHitbox().getEdges()));
        edges.addAll(Arrays.asList(otherObject.getHitbox().getEdges()));
        for (Line2D.Double edge : edges) {
            axes.add(VectorUtility.normal(VectorUtility.subtract(
                    new Vector2D(edge.getX1(), edge.getY1()),
                    new Vector2D(edge.getX2(), edge.getY2()))));
        } // end for
       
        Vector2D resultTranslation = null;
        Vector2D resultAxis = null;
        double theta;
        double translationLength;
        Pair<Double, Double> interval1, interval2;
        Vector2D transUnitVector = VectorUtility.scale(
                VectorUtility.normalize(object.getVelocity()), -1);
       
        for (Vector2D axis : axes) {
           
            interval1 = projectObject(object, axis);
            interval2 = projectObject(otherObject, axis);
            if (!(isOverlapping(interval1, interval2) &&
                    isOverlapping(interval2, interval1))) {
                // EDIT
                return null;
            } else {
               
                theta = Math.atan2(axis.getY(), axis.getX()) - Math.atan2(
                        transUnitVector.getY(), transUnitVector.getX());
                theta = correctAngle(theta);
                double a;
                if(VectorUtility.scalar(transUnitVector, axis) > 0) {
                    a = overlap(interval1, interval2);
                } else {
                    a = overlap(interval2, interval1);
                } // end if
                translationLength = a / Math.cos(theta);
                if(resultTranslation == null || translationLength <
                        VectorUtility.norm(resultTranslation)) {
                    resultTranslation = VectorUtility.scale(
                            transUnitVector, translationLength);
   
                    resultAxis = axis;
                } // end if
            } // end if
        } // end for
       
        return new Pair<>(resultAxis, resultTranslation);
    } // end collides
   
    /**
     * Projects {@code GameObject} on a line through the origin.
     * @param object - object to project.
     * @param line - line through the origin to project object onto.
     * @return the lengths of the position vectors along {@code line} to the
     * endpoints of the projection of {@code object} on {@code line}.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.1
     * @since 1.0
     * @revised Joakim Karlsson, Gustav Vidhög, 20140508
     */
    private static Pair<Double, Double> projectObject(GameObject object,
            Vector2D line) {
        Vector2D[] vertices = object.getHitbox().getVertices();
        Vector2D projection;
        double projectionNorm;
        Double low = null;
        Double high = null;
        for (Vector2D vertex : vertices) {
            vertex = VectorUtility.add(vertex, object.getPosition());
            projection = VectorUtility.project(vertex, line);
            projectionNorm =
                    Math.signum(VectorUtility.scalar(projection, line)) *
                    VectorUtility.norm(projection);
            if (low == null) {
                low = projectionNorm;
                high = projectionNorm;
            } else {
                low = Math.min(low, projectionNorm);
                high = Math.max(high, projectionNorm);
            } // end if
        } // end for
        return new Pair<>(low, high);
    } // end projectObject
   
//    private static Line2D.Double projectObjectTemp(GameObject object,
//            Vector2D line) {
//        Vector2D[] vertices = object.getHitbox().getVertices();
//        Vector2D projection;
//        double projectionNorm;
//        Double low = null;
//        Double high = null;
//        Vector2D lowVector = null;
//        Vector2D highVector = null;
//        for (Vector2D vertex : vertices) {
//            vertex = VectorUtility.add(vertex, object.getPosition());
//            projection = VectorUtility.project(vertex, line);
//            projectionNorm =
//                    Math.signum(VectorUtility.scalar(projection, line)) *
//                    VectorUtility.norm(projection);
//            if (low == null) {
//                low = projectionNorm;
//                high = projectionNorm;
//                lowVector = projection;
//                highVector = projection;
//            } else {
//                if (projectionNorm < low) {
//                    lowVector = projection;
//                }
//                if (projectionNorm > high) {
//                    highVector = projection;
//                }
//                low = Math.min(low, projectionNorm);
//                high = Math.max(high, projectionNorm);
//            }
//        }
//        return new Line2D.Double(lowVector, highVector);
//    }
   
    /**
     * Tests whether two intervals on the real number line overlaps.
     * @param interval1 - first interval to test for overlapping.
     * @param interval2 - second interval to test for overlapping.
     * @return true if intervals overlap; false otherwise.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.0
     * @since 1.0
     */
    private static boolean isOverlapping(Pair<Double, Double> interval1,
            Pair<Double, Double> interval2) {
        return overlap(interval1, interval2) != 0;
    } // end isOverlapping
    
    /**
     * Test what distance two lines have to be seperated to no longer overlap.
     * If overlapping, intervalls are to be seperated such that the first
     * interval lies closer to negative infinity than the second.
     * @param interval1 - first interval to test for overlapping.
     * @param interval2 - second interval to test for overlapping.
     * @return distance to be seperated if overlapping; 0 otherwise.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.1
     * @since 1.0
     * @revised Joakim Karlsson, Gustav Vidhög 20140508
     */
    private static double overlap(Pair<Double, Double> interval1,
            Pair<Double, Double> interval2) {
       
        double interval1max = Math.max(
                interval1.getFirst(), interval1.getSecond());
        double interval1min = Math.min(
                interval1.getFirst(), interval1.getSecond());
        double interval2max = Math.max(
                interval2.getFirst(), interval2.getSecond());
        double interval2min = Math.min(
                interval2.getFirst(), interval2.getSecond());
       
        if (interval1max <= interval2min + EPS ||
                interval2max <= interval1min + EPS) {
            return 0;
        } else {
            return (interval1max - interval2min) + EPS;
        } // end if
    } // end overlap
   
//    private static double overlapTemp(Line2D.Double interval1,
//            Line2D.Double interval2, Vector2D axis) {
//        
//        
//        
//        projectionNorm =
//                    Math.signum(VectorUtility.scalar(projection, line)) *
//                    VectorUtility.norm(projection);
//        
//        
//        double interval1max = Math.max(
//                interval1.getFirst(), interval1.getSecond());
//        double interval1min = Math.min(
//                interval1.getFirst(), interval1.getSecond());
//        double interval2max = Math.max(
//                interval2.getFirst(), interval2.getSecond());
//        double interval2min = Math.min(
//                interval2.getFirst(), interval2.getSecond());
//        
//        if (interval1max <= interval2min + EPS ||
//                interval2max <= interval1min + EPS) {
//            return 0;
//        } else {
//            return (interval1max - interval2min) + EPS;
//        }
//        throw new ThreadDeath();
//    }
   
    /**
     * Adjust angle in the interval {@code -2*Math.PI} to {@code 2*Math.PI} by
     * multiples of {@code Math.PI} to lie in the interval {@code -Math.PI/2} to
     * {@code Math.PI/2}.
     * @param theta - angle in the interval {@code -2*Math.PI} to
     * {@code 2*Math.PI}.
     * @return adjusted angle.
     * @throws IllegalArgumentException if {@code theta} is not in specified
     * interval.
     * @author Joakim Karlsson, Gustav Vidhög
     * @version 1.0
     * @since 1.2
     */
    private static double correctAngle(double theta) {
        if (theta >= -2 * Math.PI && theta < -(3/2) * Math.PI) {
            return theta + 2 * Math.PI;
        } else if (theta >= -(3/2) * Math.PI && theta < -(1/2) * Math.PI) {
            return theta + Math.PI;
        } else if (theta >= -(1/2) * Math.PI && theta <= (1/2) * Math.PI) {
            return theta;
        } else if (theta > (1/2) * Math.PI && theta <= (3/2) * Math.PI) {
            return theta - Math.PI;
        } else if (theta > (3/2) * Math.PI && theta <= 2 * Math.PI) {
            return theta - 2 * Math.PI;
        } else {
            throw new IllegalArgumentException();
        } // end if
    } // end correctAngle
} // end ObjectMover
